#!/usr/bin/env python
# -*- coding: utf-8 -*-

import envs
import rospy
import numpy as np

from geometry_msgs.msg import Twist


class RobotTwist():
    def __init__(self):
        # 小车速度发布者
        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

        # 小车直行
        self.forward_twist = Twist()
        self.forward_twist.linear.x = 0
        self.forward_twist.linear.y = 0
        self.forward_twist.linear.z = 0
        self.forward_twist.angular.x = 0
        self.forward_twist.angular.y = 0
        self.forward_twist.angular.z = 0

        # 小车旋转
        self.rotate_twist = Twist()
        self.rotate_twist.linear.x = 0
        self.rotate_twist.linear.y = 0
        self.rotate_twist.linear.z = 0
        self.rotate_twist.angular.x = 0
        self.rotate_twist.angular.y = 0
        self.rotate_twist.angular.z = 0

        # 小车停下
        self.stop_twist = Twist()
        self.stop_twist.linear.x = 0
        self.stop_twist.linear.y = 0
        self.stop_twist.linear.z = 0
        self.stop_twist.angular.x = 0
        self.stop_twist.angular.y = 0
        self.stop_twist.angular.z = 0

    def robot_twist_publish(self, twist):
        rospy.loginfo('[robot_twist] robot twist publish...')
        rate = rospy.Rate(1)
        while not rospy.is_shutdown():
            self.cmd_vel_pub.publish(self.stop_twist)
            rate.sleep()
            self.cmd_vel_pub.publish(twist)
            rate.sleep()
            self.cmd_vel_pub.publish(self.stop_twist)
            break

    def forward(self, distance):
        rospy.loginfo('[robot_twist] forward...')
        self.forward_twist.linear.x = distance
        self.robot_twist_publish(self.forward_twist)

    def rotate(self, angle):
        rospy.loginfo('[robot_twist] rotate...')
        self.rotate_twist.angular.z = angle
        self.robot_twist_publish(self.rotate_twist)


if __name__ == '__main__':
    rospy.init_node('npu_robot_twist', anonymous=True, log_level=envs.log_level)
    robot_twist = RobotTwist()
    robot_twist.forward(1.0)
    # robot_twist.rotate(np.pi / 2)
